#requires the robot team module
require 'control_lib/level1/main/robot.rb'

#requires the robot robotmap module
require 'control_lib/level1/support/robotmap.rb'

#requires the robot log module
require 'control_lib/level1/support/robotlog.rb'

#requires the math library
require 'control_lib/math/mathmodule'

#require the math library
def de2rad(float)
  rad=float
  pi=3.14159265
  if (rad>0)
    return ((2*pi)/360)*rad
  else
    return 2*pi+(((2*pi)/360)*rad)
  end
end

################ PROGRAM STARTS HERE ########################

#start the robot
robot=Robot.new("10.1.157.59",3000)
robot.init("USARBot.P2AT","2.748,-5.282,-0.446","robot1")#factory
robot.recievestart
sleep(2)

#start the robot logging
robotlog=RobotLog.new(robot,true)
robotlog.reset_logs 
robotlog.startlog

#start the map
map=Robotmap.new(robot)
map.createbeliefmap
map.autoupdatemap(true)
map.autologmap(true)
#robot.colguardmode


cbot=robot
#Main Loop
oldtime=Time.now
totaltime=0.0
movedist=3.85

cbot.smartright(6.28/4)
while(totaltime<=60.0*15.0)
  
 tforerr=0.0
 tbacerr=0.0
 
 3.times{     
  #measure forward distance
  cbot.recieve
  beforex=cbot.truepos[0].to_f
  beforey=cbot.truepos[1].to_f
  cbot.smartfor_acc(movedist,2.0)
  sleep(2.0)
  cbot.recieve
  fordist=dist(beforex,beforey,cbot.truepos[0].to_f,cbot.truepos[1].to_f)
  forerr= (((fordist-movedist).abs)/movedist)*100
  tforerr=tforerr+forerr
  puts "forward error rate=#{forerr}"

  #measure backward distance
  cbot.recieve
  beforex=cbot.truepos[0].to_f
  beforey=cbot.truepos[1].to_f
  cbot.smartback(movedist,2.0)
  sleep(2.0)
  cbot.recieve
  backdist=dist(beforex,beforey,cbot.truepos[0].to_f,cbot.truepos[1].to_f)
  backerr= (((backdist-movedist).abs)/movedist)*100
  tbacerr=tbacerr+backerr    
  puts "backward error rate=#{backerr}"
}

puts "startwriting"
open("motionbench.txt","a"){|file|
  file.puts "#{movedist},#{tforerr/3},#{tbacerr/3}"  
}
puts "writecomplete"
movedist=movedist+0.05

end  #while

sleep(20)